//
//  main.cpp
//  DCA
//
//  Created by James Critchley on 7/26/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"

#include "DCFlowControl.h"
#include "DCAComputation.h"

#include "Timer.h"

void *ThreadFunction(void *s)
{        
    DCFlowControl *dca = (DCFlowControl*) s;
           dca->assembly_traversal();                                        
    while(1){
//             dca->kinAssembly_traversal();
//             dca->kinSolve_traversal();
        dca->solve_traversal();
//             dca->null_traversal();
    }
    return 0;
}

int main (int argc, const char * argv[])
{        
        const double& q = 1.0;
        const double& u = 0.0;
        
        const int& nbodies = 16;
        const int& nproc = 4;
        const int& niter = 10000;
        int x;
        x = nbodies/nproc;        
        
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCAABA();        
        mbs3D.setQ();
        mbs3D.setU(u);
        
        DCBinaryParallelFlow dca0,dca11,dca12;        
        DCFlowTerminator dca21,dca22,dca23,dca24;
                
        dca0.child1 = &dca11;dca0.child2 = &dca12;
        
        dca11.child1 = &dca21;dca11.child2 = &dca22;
        
        dca12.child1 = &dca23;dca12.child2 = &dca24;

        CommSync sync_0_12, sync_11_22, sync_12_24;
        
        dca0.child_sync = dca12.parent_sync = &sync_0_12;
        dca11.child_sync = dca22.parent_sync = &sync_11_22;
        dca12.child_sync = dca24.parent_sync = &sync_12_24;

        // declair the calculations
        DCARoot comp0;
        DCABasic comp11, comp12;        
        DCAABA comp21,comp22,comp23,comp24;
        
        // link the computations
        dca0.computation = &comp0;
        dca11.computation = &comp11;
        dca12.computation = &comp12;
        dca21.computation = &comp21;
        dca22.computation = &comp22;
        dca23.computation = &comp23;
        dca24.computation = &comp24;
        
        // associate local data with the computations
        comp0.mbs3D = &mbs3D;
        comp0.parent_id = 4;
        comp0.child_id = 5;
        comp0.asm_id = 6;  
        comp0.calc_pinvjac = true;
                
        comp11.mbs3D = &mbs3D;
        comp11.parent_id = 0;
        comp11.child_id = 1;
        comp11.asm_id = 4;  
        comp11.calcH2 = true;        
        comp11.calc_pinvjac = true;

        comp12.mbs3D = &mbs3D;
        comp12.parent_id = 2;
        comp12.child_id = 3;
        comp12.asm_id = 5;
        comp12.calc_pinvjac = true;

        int id_count = 0;
        comp21.mbs3D = &mbs3D;
        comp21.start_id = 0;    comp21.end_id = x-1;      comp21.asm_id = 0;
        comp21.kin_startid = 0; comp21.kin_endid = x-1;
        comp21.calcH2 = true;
        comp21.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp21.kin_startid;
                
        comp22.mbs3D = &mbs3D;
        comp22.start_id = x;    comp22.end_id = 2*x-1;     comp22.asm_id = 1;        
        comp22.kin_startid = x; comp22.kin_endid = 2*x-1;       
        comp22.calcH2 = true;
        comp22.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp22.kin_startid;                        

        comp23.mbs3D = &mbs3D;
        comp23.start_id = 2*x;   comp23.end_id = 3*x-1;     comp23.asm_id = 2;
        comp23.kin_startid = 2*x; comp23.kin_endid = 3*x-1;
        comp23.calcH2 = true;
        comp23.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp23.kin_startid;                
        
        comp24.mbs3D = &mbs3D;
        comp24.start_id = 3*x;   comp24.end_id = nbodies-1;     comp24.asm_id = 3;        
        comp24.kin_startid = 3*x; comp24.kin_endid = 4*x-1;        
        comp24.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp24.kin_startid;
        mbs3D.ids_[id_count++] = comp24.kin_endid+1;
                
        comp0.calcH2 = comp12.calcH2 = comp24.calcH2 = true;
                                
        pthread_attr_t attr;
        pthread_t thread1,thread2,thread3;

        pthread_attr_init(&attr);                
        pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);        
                
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                
        void *data = (void *) &dca12;
        pthread_create(&thread1, &attr, ThreadFunction, data);
        data = (void *) &dca22;
        pthread_create(&thread2, &attr, ThreadFunction, data);
        data = (void *) &dca24;
        pthread_create(&thread3, &attr, ThreadFunction, data);
        
        //clean-up the attributes
        pthread_attr_destroy(&attr);
        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
        mbs3D.calcJ();
        dca0.assembly_traversal();
        RSIM::Timer t;
        for(int i=0;i<niter;i++){        
                // run the root subsystem
//                 dca0.kinAssembly_traversal();
//                 dca0.kinSolve_traversal();
                     dca0.solve_traversal();
//                 dca0.null_traversal();
        }    
        t.stop();        
        t.printElapsedTime(0);
//         cout<<mbs3D.pinvjac_<<endl;
//         cout<<mbs3D.null_<<endl;
//         mbs3D.solveSystemHandleEquationsCase_OpControl();
//         cout<<mbs3D.iLambda_<<endl;
//         cout<<mbs3D.pe_<<endl;

        exit(0);
        return 0;
}
